#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>

namespace AD{
    using namespace cv;
    class Armor_Detector{
        private:
            bool If_Detect,If_CalcDis,If_DrawDis;
            Mat src,img,binary_img;
            Mat cameraMatrix,distCoeffs;
            std::vector<Point3f> armorSize,armorInCamera;
            std::vector<std::vector<Point> > contours;
            std::vector<RotatedRect> light_bar;
            std::vector<std::vector<Point2f> >armor;
            const double PI=3.1415926;
            void detect();
            void Threshold();
            void find_light_bar();
            void find_armor();
            void draw_armor(RotatedRect &a, RotatedRect &b);
            void draw_light_bar(RotatedRect &rect);
            double get_angle(Point2f a,Point2f b);
            void calc_dis();
            void draw_dis();
        public:
            Armor_Detector(const Mat &g){   //初始化
                src=g;
                If_Detect=false;
                If_CalcDis=false;
                If_DrawDis=false;
                armorSize.push_back(Point3f(-7, -3, 0));
                armorSize.push_back(Point3f( 7, -3, 0));
                armorSize.push_back(Point3f( 7,  3, 0));
                armorSize.push_back(Point3f(-7,  3, 0));
                cameraMatrix=(Mat_<double>(3,3) << 
                    1000, 0, src.cols/2.0,
                    0, 1000, src.rows/2.0,
                    0, 0, 1
                );
                distCoeffs=(Mat_<double>(5,1) << 0,0,0,0,0);
                armor.clear();
                light_bar.clear();
                armorInCamera.clear();
            }
            void Detect(){                  //在图片上识别装甲板
                if(!If_Detect)detect();
            }
            void drawDis(){                 //在图片上标记距离
                if(!If_CalcDis)calc_dis();
                if(!If_DrawDis)draw_dis();
            }
            void getPos(std::vector<Point2f> &Armor,std::vector<Point3f> &Coordinate){  //获取装甲板在相机坐标系的位置
                if(!If_CalcDis)calc_dis();
                Armor.resize(armor.size());
                for(int i=0;i<armor.size();i++){
                    Armor[i]=(armor[i][0]+armor[i][2])/2;
                }
                Coordinate=armorInCamera;
            }
            void inputCameraParameter(const Mat &cameraMatrix_,const Mat &distCoeffs_){     //导入相机参数
                cameraMatrix=cameraMatrix_;
                distCoeffs=distCoeffs_;
            }

            void operator << (const Mat &g){        //输入图片
                src=g;
                If_Detect=false;
                If_CalcDis=false;
                If_DrawDis=false;
                armor.clear();
                light_bar.clear();
                armorInCamera.clear();
            }
            void operator >> (Mat &g){              //输出图片
                g=img;
            }
    };
}